#include "PID.h"

//--------------------- PID --------------------//

//PID基本参数
static int control_period_ms = 0;     // 控制周期, 与定时器中断时间间隔一致

float control_period_s;           // 控制周期,秒

/**
 * @brief       初始化PID控制器
 * @param pid   传入PID控制器结构体指针
 * @param kp    Kp参数,float
 * @param ki    Ki参数,float
 * @param kd    Kd参数,float
 * @param max_limit 输出值的最大极限
 * @param min_limit 输出值的最小极限
 * @param period_ms 控制周期(毫秒)
 * @return int   0代表成功，1代表失败
 */
int PID_Init(PID_Controller* pid,
             float kp, float ki, float kd, 
             float max_limit, float min_limit, 
             int period_ms)
{
    // 检查参数正确性
    if (max_limit <= min_limit)
    {
        return 1;
    }
    

    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->target = 0.0f;
    pid->actual = 0.0f;
    pid->error = 0.0f;
    pid->last_error = 0.0f;
    pid->integral = 0.0f;
    pid->output_limit_max = max_limit;
    pid->output_limit_min = min_limit;

    control_period_ms = period_ms;
	
	// 转换周期单位
	control_period_s = ((float)control_period_ms)/1000;     // 控制周期, 20毫秒

    // 计算输出偏移
    pid->output_move = (pid->output_limit_max + pid->output_limit_min)/2;
    return 0;
}


/**
 * @brief 计算并返回调控量(位置式PID)
 * @note  通过修改kp以达到输入电压输出duty
 * @param pid 传入PID控制器结构体指针
 * @param target 目标参量
 * @param actual 实际参量
 * @return float (Duty)
 */
float PID_Calculate(PID_Controller* pid, float target, float actual)
{
    pid->target = target;
    pid->actual = actual;
    pid->error = pid->target - pid->actual;

    // 积分项，带抗饱和处理
    pid->integral += pid->error;

    float integral_output = pid->Ki * pid->integral * control_period_s;// Ki需要乘以时间
    if (integral_output > pid->output_limit_max)
    {
        integral_output = pid->output_limit_max;
    } 
    else if (integral_output < pid->output_limit_min) 
    {
        integral_output = pid->output_limit_min;
    }

    // 两次误差之差
    float derivative = pid->error - pid->last_error;
	
    float output = (float)(pid->Kp * pid->error +
                           pid->Ki * integral_output + 
                           pid->Kd * derivative / control_period_s);    // Kd需要除以时间

    pid->last_error = pid->error;

    // 输出限幅
    output > pid->output_limit_max? output = pid->output_limit_max:output;
    output < pid->output_limit_min? output = pid->output_limit_min:output;

    return output;
}
